#include <TL/robotManipulationDomain.h>


RobotManipulationSimulator sim;


void initSimulator(const char* configurationFile, bool takeMovie) {
	sim.shutdownAll();
	sim.loadConfiguration(configurationFile);
#ifdef MT_FREEGLUT
	orsDrawProxies = false;
	orsDrawJoints = false;
#endif
	sim.startOde();

	//sim.initWorldEditor();

	if (takeMovie)
		sim.startRevel();
	sim.startSwift();
	sim.simulate(50);
}


std::string removeBreaks(const std::string &str) {
	std::string result = str;
	while (int pos = result.find("\n") != std::string::npos)
		result = result.replace(pos, 1, "");
	return result;
}


void test(int argn,char** argv) {
	uint randSeed = 12345;
	rnd.seed(randSeed);

	// Set up logic
	TL::logicObjectManager::setPredicatesAndFunctions("language.dat");
	TL::logicObjectManager::writeLanguage("used_language.dat");

	MT::String sim_file("../../configurations/situation.ors");
	initSimulator(sim_file, false);
	sim.simulate(50);

	// Get objects and set in logic database
	uintA objs;
	sim.getObjects(objs);
	TL::logicObjectManager::setConstants(objs);


	// Sequence of actions to perform
	AtomA actions;
	TL::logicObjectManager::getAtoms(actions, "grab(67) puton(73) grab(66) puton(68) grab(67) puton(73) grab(67) puton(65) grab(67) puton(72) grab(67) puton(65) grab(67) puton(72) grab(65) puton(73)");
	//TL::logicObjectManager::getAtoms(actions, "grab(65) puton(72) grab(65) puton(72) grab(67) puton(73) grab(67) puton(73) grab(66) puton(72) grab(66) puton(73)");
	cout<<"Actions: "<<actions<<endl;

	std::ofstream sampleFile("scaleSamples.dat");

	sampleFile << "[";

	uint i;
	FOR1D(objs, i) {
		sampleFile << objs(i);
		if (i != objs.N-1) sampleFile << " ";
	}

	sampleFile << "]" << std::endl;

	// Perform

	FOR1D(actions, i) {
		cout<<"TIME-STEP t="<<i<<endl;
		// OBSERVE STATE
		TL::State* s = TL::RobotManipulationDomain::observeLogic(&sim);

		sampleFile << std::endl;
		sampleFile << *s << std::endl;

		cout<<endl<<"OBSERVED STATE:"<<endl<<*s<<endl;
		//cout<<"Please press button to continue."<<endl;
		//sim.watch();
		// ACTION
		sampleFile << *actions(i) << std::endl;
		cout<<endl<<"ACTION #"<<i<<" "<<*actions(i)<<endl;
		TL::RobotManipulationDomain::performAction(actions(i), &sim, 200);
		cout<<"----------------------"<<endl;
	}

	// OBSERVE STATE
	TL::State* s = TL::RobotManipulationDomain::observeLogic(&sim);

	sampleFile << std::endl;
	sampleFile << *s << std::endl;

	sampleFile.close();

	cout<<"Please press button to continue."<<endl;
	sim.watch();

	sim.shutdownAll();
}


int main(int argc, char** argv){
	test(argc, argv);
	return 0;
}
